#pragma once

#include <string>
#include <vector>
#include <string.h>

#define CHECK_AND_RESIZE(size)                           \
    if (offset_ + size > size_)                          \
    {                                                    \
        ReAllocate((size_ + size + 4096) / 4096 * 4096); \
    }

#define PT_PUT_UINT8(v) \
    data_[offset_++] = v & 0xff;

#define PT_PUT_UINT16(v) \
    PT_PUT_UINT8(v);     \
    PT_PUT_UINT8(v >> 8);

#define PT_PUT_UINT32(v) \
    PT_PUT_UINT16(v);    \
    PT_PUT_UINT16(v >> 16);

#define PT_PUT_UINT64(v) \
    PT_PUT_UINT32(v);    \
    PT_PUT_UINT32(v >> 32);

struct ros_time
{
    uint32_t nsecs;
    uint32_t secs;
};

class PackageTool
{
public:
    PackageTool(uint8_t *data = nullptr, uint32_t size = 4096)
    {
        ReAllocate(size);
        if (data != nullptr)
        {
            memcpy(data_, data, size);
        }
    }
    ~PackageTool()
    {
        if (data_ != nullptr)
        {
            delete data_;
            data_ = nullptr;
        }
    }
    // bool
    // byte
    // char
    // duration
    // float32
    // float64
    // int16
    // int32
    // int64
    // int8
    // string
    // time
    // uint16
    // uint32
    // uint64
    // uint8
    void put_bool(bool value)
    {
        CHECK_AND_RESIZE(2);
        PT_PUT_UINT8(value ? 1 : 0);
    }
    void put_byte(uint8_t value)
    {
        CHECK_AND_RESIZE(1);
        PT_PUT_UINT8(value);
    }
    void put_char(char value)
    {
        CHECK_AND_RESIZE(1);
        PT_PUT_UINT8(value);
    }
    void put_duration(struct ros_time &value)
    {
        CHECK_AND_RESIZE(8);
        PT_PUT_UINT32(value.secs);
        PT_PUT_UINT32(value.nsecs);
    }
    void put_float32(float value)
    {
        CHECK_AND_RESIZE(4);
        PT_PUT_UINT32(*reinterpret_cast<uint32_t *>(&value));
    }
    void put_float64(double value)
    {
        CHECK_AND_RESIZE(8);
        PT_PUT_UINT64(*reinterpret_cast<uint64_t *>(&value));
    }
    void put_int16(int16_t value)
    {
        CHECK_AND_RESIZE(2);
        PT_PUT_UINT16(value);
    }
    void put_int32(int32_t value)
    {
        CHECK_AND_RESIZE(4);
        PT_PUT_UINT32(value);
    }
    void put_int64(int64_t value)
    {
        CHECK_AND_RESIZE(8);
        PT_PUT_UINT64(value);
    }
    void put_int8(int8_t value)
    {
        CHECK_AND_RESIZE(1);
        PT_PUT_UINT8(value);
    }
    void put_string(const std::string &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + size);
        PT_PUT_UINT32(size);
        memcpy(data_ + offset_, value.data(), size);
        offset_ += size;
    }
    void put_time(struct ros_time &value)
    {
        put_duration(value);
    }
    void put_uint16(uint16_t value)
    {
        CHECK_AND_RESIZE(2);
        PT_PUT_UINT16(value);
    }
    void put_uint32(uint32_t value)
    {
        CHECK_AND_RESIZE(4);
        PT_PUT_UINT32(value);
    }
    void put_uint64(uint64_t value)
    {
        CHECK_AND_RESIZE(8);
        PT_PUT_UINT64(value);
    }
    void put_uint8(uint8_t value)
    {
        CHECK_AND_RESIZE(1);
        PT_PUT_UINT8(value);
    }

    void put_bool_array(const std::vector<bool> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            data_[offset_++] = v ? 1 : 0;
        }
    }
    void put_byte_array(const std::vector<uint8_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            data_[offset_++] = v;
        }
    }
    void put_char_array(const std::vector<char> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            data_[offset_++] = v;
        }
    }
    void put_duration_array(const std::vector<struct ros_time> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 8 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT32(v.secs);
            PT_PUT_UINT32(v.nsecs);
        }
    }
    void put_float32_array(const std::vector<float> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 4 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT32(*reinterpret_cast<uint32_t *>(&v));
        }
    }
    void put_float64_array(const std::vector<double> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 8 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT64(*reinterpret_cast<uint64_t *>(&v));
        }
    }
    void put_int16_array(const std::vector<int16_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 2 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT16(v);
        }
    }
    void put_int32_array(const std::vector<int32_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 4 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT32(v);
        }
    }
    void put_int64_array(const std::vector<int64_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 8 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT64(*reinterpret_cast<uint64_t *>(&v));
        }
    }
    void put_int8_array(const std::vector<int8_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            data_[offset_++] = v;
        }
    }
    void put_string_array(const std::vector<std::string> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 4 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT32(v.size());
            memcpy(data_ + offset_, v.data(), v.size());
            offset_ += v.size();
        }
    }
    void put_time_array(const std::vector<struct ros_time> &value)
    {
        put_duration_array(value);
    }
    void put_uint16_array(const std::vector<uint16_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 2 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            data_[offset_++] = v & 0xff;
            data_[offset_++] = (v >> 8) & 0xff;
        }
    }
    void put_uint32_array(const std::vector<uint32_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 4 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT32(v);
        }
    }
    void put_uint64_array(const std::vector<uint64_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + 8 * size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            PT_PUT_UINT64(v);
        }
    }
    void put_uint8_array(const std::vector<uint8_t> &value)
    {
        uint32_t size = value.size();
        CHECK_AND_RESIZE(4 + size);
        PT_PUT_UINT32(size);
        for (auto v : value)
        {
            data_[offset_++] = v;
        }
    }

    bool get_bool()
    {
        return data_[offset_++] != 0;
    }
    uint8_t get_byte()
    {
        return data_[offset_++];
    }
    char get_char()
    {
        return data_[offset_++];
    }
    struct ros_time get_duration()
    {
        struct ros_time value;
        value.secs = get_uint32();
        value.nsecs = get_uint32();
        return value;
    }
    float get_float32()
    {
        uint32_t value = get_uint32();
        return *reinterpret_cast<float *>(&value);
    }
    double get_float64()
    {
        uint64_t value = get_uint64();
        return *reinterpret_cast<double *>(&value);
    }
    int16_t get_int16()
    {
        return get_uint16();
    }
    int32_t get_int32()
    {
        return get_uint32();
    }
    int64_t get_int64()
    {
        return get_uint64();
    }
    int8_t get_int8()
    {
        return data_[offset_++];
    }
    std::string get_string()
    {
        uint32_t size = get_uint32();
        std::string value((char *)(data_ + offset_), size);
        offset_ += size;
        return value;
    }
    struct ros_time get_time()
    {
        return get_duration();
    }
    uint16_t get_uint16()
    {
        uint16_t value = data_[offset_++];
        value |= data_[offset_++] << 8;
        return value;
    }
    uint32_t get_uint32()
    {
        uint32_t value = data_[offset_++];
        value |= data_[offset_++] << 8;
        value |= data_[offset_++] << 16;
        value |= data_[offset_++] << 24;
        return value;
    }
    uint64_t get_uint64()
    {
        uint64_t value = get_uint32();
        value |= (uint64_t)get_uint32() << 32;
        return value;
    }
    uint8_t get_uint8()
    {
        return data_[offset_++];
    }

    std::vector<bool> get_bool_array()
    {
        uint32_t size = get_uint32();
        std::vector<bool> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = data_[offset_++] != 0;
        }
        return value;
    }
    std::vector<uint8_t> get_byte_array()
    {
        uint32_t size = get_uint32();
        std::vector<uint8_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = data_[offset_++];
        }
        return value;
    }
    std::vector<char> get_char_array()
    {
        uint32_t size = get_uint32();
        std::vector<char> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = data_[offset_++];
        }
        return value;
    }
    std::vector<struct ros_time> get_duration_array()
    {
        uint32_t size = get_uint32();
        std::vector<struct ros_time> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i].secs = get_uint32();
            value[i].nsecs = get_uint32();
        }
        return value;
    }
    std::vector<float> get_float32_array()
    {
        uint32_t size = get_uint32();
        std::vector<float> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            uint32_t v = get_uint32();
            value[i] = *reinterpret_cast<float *>(&v);
        }
        return value;
    }
    std::vector<double> get_float64_array()
    {
        uint32_t size = get_uint32();
        std::vector<double> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            uint64_t v = get_uint64();
            value[i] = *reinterpret_cast<double *>(&v);
        }
        return value;
    }
    std::vector<int16_t> get_int16_array()
    {
        uint32_t size = get_uint32();
        std::vector<int16_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = get_uint16();
        }
        return value;
    }
    std::vector<int32_t> get_int32_array()
    {
        uint32_t size = get_uint32();
        std::vector<int32_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = get_uint32();
        }
        return value;
    }
    std::vector<int64_t> get_int64_array()
    {
        uint32_t size = get_uint32();
        std::vector<int64_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = get_uint64();
        }
        return value;
    }
    std::vector<int8_t> get_int8_array()
    {
        uint32_t size = get_uint32();
        std::vector<int8_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = data_[offset_++];
        }
        return value;
    }
    std::vector<std::string> get_string_array()
    {
        uint32_t size = get_uint32();
        std::vector<std::string> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            uint32_t strSize = get_uint32();
            value[i].assign((char *)(data_ + offset_), strSize);
            offset_ += strSize;
        }
        return value;
    }
    std::vector<struct ros_time> get_time_array()
    {
        return get_duration_array();
    }
    std::vector<uint16_t> get_uint16_array()
    {
        uint32_t size = get_uint32();
        std::vector<uint16_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = get_uint16();
        }
        return value;
    }
    std::vector<uint32_t> get_uint32_array()
    {
        uint32_t size = get_uint32();
        std::vector<uint32_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = get_uint32();
        }
        return value;
    }
    std::vector<uint64_t> get_uint64_array()
    {
        uint32_t size = get_uint32();
        std::vector<uint64_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = get_uint64();
        }
        return value;
    }
    std::vector<uint8_t> get_uint8_array()
    {
        uint32_t size = get_uint32();
        std::vector<uint8_t> value(size);
        for (uint32_t i = 0; i < size; i++)
        {
            value[i] = data_[offset_++];
        }
        return value;
    }

    uint8_t *data() const
    {
        return data_;
    }
    uint32_t size() const
    {
        return size_;
    }
    uint32_t offset(uint32_t offset = 0xffffffff)
    {
        if (offset != 0xffffffff)
        {
            offset_ = offset;
        }
        return offset_;
    }

private:
    void ReAllocate(uint32_t size)
    {
        uint8_t *newdata = new uint8_t[size];
        if (data_ != nullptr)
        {
            if (offset_ > 0)
            {
                memcpy(newdata, data_, offset_);
            }
            delete data_;
            data_ = nullptr;
        }
        data_ = newdata;
        size_ = size;
    }
    uint8_t *data_ = nullptr;
    uint32_t size_ = 0;
    uint32_t offset_ = 0;
};

// /**
//  * PackageTool 需要单元测试
//  */

// #include <map>

// class MaestroParam
// {
// public:
//     std::string type;   //比如int8或者复合类型
//     std::string name;   //参数名
//     bool isArray;      //是否是数组
//     uint32_t arraySize; //数组大小
//     MaestroMsg *pmsg;   //如果是复合类型，指向该类型
// };

// class MaestroMsg
// {
// public:
//     void Expand()
//     { //打开msg文件，解析每一行，填充params_
//         /*
//         for(auto line : lines_)
//         {
//             type , name , isArray , arraySize = line;
//             if(type is 基础类型){
//                 params_.push_back(MaestroParam(type, name, isArray, arraySize, nullptr));
//             } else {//复合类型
//                 nodeName , msgName = type;
//                 MaestroNode *pnode = MaestroNodeManager::GetInstance().FindNode(nodeName);
//                 MaestroMsg *pmsg = pnode->FindMsg(msgName);
//                 params_.push_back(MaestroParam(type, name, isArray, arraySize, pmsg));
//             }
//         }
//         */
//     }

// private:
//     std::string name_;
//     std::string path_;                           //执行msg文件路径
//     std::map<std::string, MaestroParam> params_; //子消息列表
// };

// class MaestroService
// {
// public:
// private:
// };

// class MaestroNode
// {
// public:
//     //获取名字
//     const std::string &GetName() const
//     {
//         return name_;
//     }
//     //查找子消息
//     MaestroMsg *FindMsg(const std::string &name)
//     {
//     }

// private:
//     std::vector<MaestroMsg> msgs_;
//     std::vector<MaestroService> services_;
//     std::string name_;
// };

// class MaestroNodeManager
// {
// public:
//     MaestroNodeManager()
//     { //通过解析 /opt/ros/noetic/share 下面的node，填充nodes
//     }
//     //根据名称查找节点
//     MaestroNode *FindNode(const std::string &name)
//     {
//         for (auto &node : nodes_)
//         {
//             if (node.GetName() == name)
//             {
//                 return &node;
//             }
//         }
//         return nullptr;
//     }

// private:
//     std::vector<MaestroNode> nodes_;
// };

// void main()
// {
//     MaestroNodeManager manager;
// }
